#ifndef HERMITE_INTERPOLATION_H
#define HERMITE_INTERPOLATION_H

#include <vector>
#include <iostream>
#include <iomanip>
#include <cmath>

using namespace std;

// 定义一个结构体来存储位置和速度
struct PositionAndVelocity {
    double position;
    double velocity;
};

// 计算Hermite插值多项式的函数，直接返回插值结果(仅适用于二阶导数)
PositionAndVelocity hermiteInterpolation(const vector<double> &nodes, const vector<double> &func_values, const vector<double> &deriv_values, double x)
{
    size_t n = nodes.size();
    size_t m = 2 * n;
    vector<vector<double>> table(m, vector<double>(m, 0.0)); // 创建一个下三角矩阵差商表

    vector<double> allnodes;
    allnodes.reserve(2 * n); // 预留足够的空间
    for (size_t i = 0; i < n; ++i)
    {
        allnodes.push_back(nodes[i]);
        allnodes.push_back(nodes[i]);
    }

    // 构建差商表
    for (size_t i = 0; i < n; i++)
    {
        table[2 * i][0] = func_values[i];
        table[2 * i + 1][0] = func_values[i]; // 将节点处的函数值存放在差商表的第一列
    }
    for (size_t j = 1; j < m; j++)
    {
        for (size_t i = j; i < m; i++)
        {
            if (j == 1)
            {
                if (i % 2 == 1)
                {
                    table[i][j] = deriv_values[(i - 1) / 2]; // 第一阶导数值
                }
                else
                {
                    table[i][j] = (table[i][j - 1] - table[i - 1][j - 1]) / (allnodes[i] - allnodes[i - 1]);
                }
            }
            else
            {
                table[i][j] = (table[i][j - 1] - table[i - 1][j - 1]) / (allnodes[i] - allnodes[i - j]);
            }
        }
    }

    // 计算Hermite插值多项式的值和导数
    double result = table[0][0]; // 从差商表的第一个元素开始
    double velocity = 0.0; // 速度
    double mult = 1.0;
    double dmult = 0.0;
    for (size_t i = 1; i < m; i++)
    {   
        dmult = dmult * (x - allnodes[i - 1]) + mult;
        velocity += table[i][i] * dmult;
        mult *= (x - allnodes[i - 1]);
        result += table[i][i] * mult;
    }
    return {result, velocity};
}

#endif // HERMITE_INTERPOLATION_H